'''夹具部分，接收循迹或蓝牙模式发来的指令进行夹具的夹取和松开运动，循迹模式调用夹取或松开函数会自动检测是否到达夹具终点'''
from machine import Pin,Timer

#夹具电机端口
LeftMotor1=Pin(Pin('B12'),Pin.OUT)
LeftMotor2=Pin(Pin('B13'),Pin.OUT)
RightMotor1=Pin(Pin('B14'),Pin.OUT)
RightMotor2=Pin(Pin('B15'),Pin.OUT)
#夹具检测开关
LeftTakeKey=Pin(Pin('A4'),Pin.IN,Pin.PULL_DOWN) #左边夹紧时的行程开关
RightTakeKey=Pin(Pin('A5'),Pin.IN,Pin.PULL_DOWN) #右边夹紧时的行程开关
LeftPutKey=Pin(Pin('A6'),Pin.IN,Pin.PULL_DOWN) #左边松开时的行程开关
RightPutKey=Pin(Pin('A7'),Pin.IN,Pin.PULL_DOWN) #右边松开时的行程开关

#套箱升降电机端口
UpDownMotor1=Pin(Pin('C0'),Pin.OUT)
UpDownMotor2=Pin(Pin('C1'),Pin.OUT)
#套箱升降检测开关
BoxUpKey=Pin(Pin('C2'),Pin.OUT)#升到最顶端开关
BoxDownKey=Pin(Pin('C3'),Pin.OUT)#降到最低端开关
BoxGoKey=Pin(Pin('C4'),Pin.OUT)#行走时提起一点高度的开关
BoxInKey=Pin(Pin('C5'),Pin.OUT)#箱子是否进到套箱内的检测开关

'''
#夹具夹紧松开
def Take():#夹紧
    LeftMotor1.value(1)
    LeftMotor2.value(0)
    RightMotor1.value(1)
    RightMotor2.value(0)
    while 1:
        if LeftTakeKey.value():#如果两边的开关都按下 接VCC，变成1就停
            Stop()
        if RightTakeKey.value():#如果两边的开关都按下 接VCC，变成1就停
            Stop()
        if LeftPutKey.value() and RightPutKey.value():#再次确认两边都按倒开关了就退出循环
            return 1
def Put():#松开
    LeftMotor1.value(0)
    LeftMotor2.value(1)
    RightMotor1.value(0)
    RightMotor2.value(1)
    while 1:
        if LeftPutKey.value():#如果两边的开关都按下 接地，变成0就停
            Stop()
        if RightPutKey.value():#如果两边的开关都按下 接地，变成0就停
            Stop()
        if LeftPutKey.value() and RightPutKey.value():#再次确认两边都按倒开关了就退出循环
            return 1
def Stop():#急停
    LeftMotor1.value(1)
    LeftMotor2.value(1)
    RightMotor1.value(1)
    RightMotor2.value(1)

#读取所有行程开关信号值
def SwitchSignal():#夹具4个行程开关信号
    return [LeftTakeKey,RightTakeKey,LeftPutKey,RightPutKey]

def Fixture(YN):#夹具夹取松开
    #如果调用夹具，传入信号True为夹取，Flase为松开
    if YN:#1为夹取
        Take()
        Stop()
    else:#0为松开
        Put()
        Stop()
    return 1
'''

def Stop():#急停
    print('Fixture Stop')
    LeftMotor1.value(1)
    LeftMotor2.value(1)
    RightMotor1.value(1)
    RightMotor2.value(1)
#夹具夹紧松开测试
def TestTake():#夹紧
    print('Fixture Take')
    LeftMotor1.value(1)
    LeftMotor2.value(0)
    RightMotor1.value(1)
    RightMotor2.value(0)
    return 1
def TestPut():#松开
    print('Fixture Put')
    LeftMotor1.value(0)
    LeftMotor2.value(1)
    RightMotor1.value(0)
    RightMotor2.value(1)
    return 1

#套箱升降
def TestUpDownStop():#急停
    print('Box Stop')
    UpDownMotor1.value(1)
    UpDownMotor2.value(1)

def TestUp(where):#套箱升起
    print('Box Up')
    UpDownMotor1.value(1)
    UpDownMotor2.value(0)
    if where=='top':
        while 1:
            if BoxUpKey.value():#升到最顶端尽头 接VCC，变成1就停
                TestDown()
                delay(10)
                TestUpDownStop()
                break
    elif where=='bit':
        while 1:
            if BoxGoKey.value():#升起一段距离行走 接VCC，变成1就停
                TestDown()
                delay(10)
                TestUpDownStop()
                break
    return 1
def TestDown():#套箱下降
    print('Box Down')
    UpDownMotor1.value(0)
    UpDownMotor2.value(1)
    if where=='bottom':
        while 1:
            if BoxDownKey.value():#降到最底端尽头 接VCC，变成1就停
                TestUp()
                delay(10)
                TestUpDownStop()
                break
    elif where=='bit':
        while 1:
            if BoxGoKey.value():#升起一段距离行走 接VCC，变成1就停
                TestUp()
                delay(10)
                TestUpDownStop()
                break
    return 1

#套箱升降测试
def TestUpDownStop():#急停
    print('Box Stop')
    UpDownMotor1.value(1)
    UpDownMotor2.value(1)

def TestUp():#夹紧
    print('Box Up')
    UpDownMotor1.value(1)
    UpDownMotor2.value(0)
    return 1
def TestDown():#松开
    print('Box Down')
    UpDownMotor1.value(0)
    UpDownMotor2.value(1)
    return 1
